﻿#include <math.h>
#include <float.h>
#include <stdint.h>
#include <assert.h>

inline PTSIMDVector PT_VECTORCALL PTSIMDV3FLoad(const PTV3F *pSource)
{
	__m128 vX = ::_mm_load_ss( &pSource->x );//x _ _ _
    __m128 vY = ::_mm_load_ss( &pSource->y );//y _ _ _
    __m128 vZ = ::_mm_load_ss( &pSource->z );//z _ _ _

	PTSIMDVector vResult;
	vResult.v = ::_mm_unpacklo_ps(vX, vY);//x y _ _
	vResult.v = ::_mm_shuffle_ps(vResult.v, vZ, _MM_SHUFFLE(0, 0, 1, 0));//x y z _
	return vResult;
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV3FLoadA(const PTV3F *pSource)
{
	assert((reinterpret_cast<uintptr_t>(pSource) % 16U) == 0U);

	PTSIMDVector vResult;
	vResult.v = ::_mm_load_ps(&pSource->x);
	return vResult;
}

inline void PT_VECTORCALL PSIMDV3FStore(PTV3F* pDestination, PTSIMDVector V)
{
	__m128 vY = ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	__m128 vZ = ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	_mm_store_ss(&pDestination->x, V.v);
	_mm_store_ss(&pDestination->y, vY);
	_mm_store_ss(&pDestination->z, vZ);
}

inline void PT_VECTORCALL PSIMDV3FStoreA(PTV3F* pDestination, PTSIMDVector V)
{
	assert((reinterpret_cast<uintptr_t>(pDestination) % 16U) == 0U);

	::_mm_storel_epi64(reinterpret_cast<__m128i*>(pDestination), ::_mm_castps_si128(V.v));
	::_mm_store_ss(&pDestination->z, ::_mm_shuffle_ps(V.v, V.v, _MM_SHUFFLE(2, 2, 2, 2)));
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV3FDot(PTSIMDVector V1, PTSIMDVector V2)
{
	//映射
	__m128 vTemp1 = _mm_mul_ps(V1.v, V2.v);
	//归约
	__m128 vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z y _ _ 注意是ss而不是ps!!!
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y _ _ _
	
	PTSIMDVector vResult;
	vResult.v = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y+w x+z+y+w x+z+y+w x+z+y+w
	return vResult;
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV3FCross(PTSIMDVector V1, PTSIMDVector V2)
{
	PTSIMDVector vResult;
	vResult.v = _mm_sub_ps(//y1z2-z1y2 z1x2-x1z2 x1y2-y1x2
		::_mm_mul_ps(//y1z2 z1x2 x1y2
			::_mm_shuffle_ps(V1.v, V1.v, _MM_SHUFFLE(0, 0, 2, 1)),//y1 z1 x1 _
			::_mm_shuffle_ps(V2.v, V2.v, _MM_SHUFFLE(0, 1, 0, 2))//z2 x2 y2 _
		),
		::_mm_mul_ps(//z1y2 x1z2 y1x2
			::_mm_shuffle_ps(V1.v, V1.v, _MM_SHUFFLE(0, 1, 0, 2)),//z1 x1 y1 _
			::_mm_shuffle_ps(V2.v, V2.v, _MM_SHUFFLE(0, 0, 2, 1))//y2 z2 x2 _
		)
	);
	return vResult;
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV3FNormalize(PTSIMDVector V)
{
	PTSIMDVector vLengthSquare = ::PTSIMDV3FDot(V, V);
	
	__m128 vLength = ::_mm_sqrt_ps(vLengthSquare.v);

	//零向量的方向是任意的！！！

	PTSIMDVector vResult;
	vResult.v = ::_mm_div_ps(V.v, vLength);
	return vResult;
}


inline PTSIMDVector PT_VECTORCALL PTSIMDV4FLoad(const PTV4F *pSource)
{
	PTSIMDVector vResult;
	vResult.v = ::_mm_loadu_ps(&pSource->x);
	return vResult;
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV4FLoadA(const PTV4F *pSource)
{
	assert((reinterpret_cast<uintptr_t>(pSource) % 16U) == 0U);

	PTSIMDVector vResult;
	vResult.v = ::_mm_load_ps(&pSource->x);
	return vResult;
}

inline void PT_VECTORCALL PTSIMDV4FStore(PTV4F* pDestination, PTSIMDVector V)
{
	::_mm_storeu_ps(&pDestination->x, V.v);
}

inline void PT_VECTORCALL PTSIMDV4FStoreA(PTV4F* pDestination, PTSIMDVector V)
{
	assert((reinterpret_cast<uintptr_t>(pDestination) % 16U) == 0U);

	::_mm_store_ps(&pDestination->x, V.v);
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV4FNegate(PTSIMDVector V)
{
	__m128 vZero = ::_mm_setzero_ps();
	PTSIMDVector vResult;
	vResult.v = ::_mm_sub_ps(vZero, V.v);
	return vResult;
}

inline PTSIMDVector PT_VECTORCALL PTSIMDV4FDot(PTSIMDVector V1, PTSIMDVector V2)
{
	//映射
	__m128 vTemp1 = _mm_mul_ps(V1.v, V2.v);
	//归约
	__m128 vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 3, 2));//z w _ _
	vTemp1 = ::_mm_add_ps(vTemp1, vTemp2);//x+z y+w _ _ 
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y+w _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y+w _ _ _ 注意是ss而不是ps!!!

	PTSIMDVector vResult;
	vResult.v = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y+w x+z+y+w x+z+y+w x+z+y+w
	return vResult;
}


inline PTSIMDVector PT_VECTORCALL PTSIMDV4FTransform(PTSIMDMatrix M, PTSIMDVector V)
{
	PTSIMDMatrix mTemp;
	//映射
	mTemp.r[0] = ::_mm_mul_ps(M.r[0], V.v);
	mTemp.r[1] = ::_mm_mul_ps(M.r[1], V.v);
	mTemp.r[2] = ::_mm_mul_ps(M.r[2], V.v);
	mTemp.r[3] = ::_mm_mul_ps(M.r[3], V.v);

	//归约
	mTemp = ::PTSIMDM44FTranspose(mTemp);
	PTSIMDVector vResult;
	vResult.v = ::_mm_add_ps(::_mm_add_ps(mTemp.r[0], mTemp.r[1]), ::_mm_add_ps(mTemp.r[2], mTemp.r[3]));

	return vResult;
}

inline void PT_VECTORCALL PTSIMDM44FStore(PTM44F* pDestination, PTSIMDMatrix M)
{
	::_mm_storeu_ps(&pDestination->r[0].x, M.r[0]);
	::_mm_storeu_ps(&pDestination->r[1].x, M.r[1]);
	::_mm_storeu_ps(&pDestination->r[2].x, M.r[2]);
	::_mm_storeu_ps(&pDestination->r[3].x, M.r[3]);
}

inline void PT_VECTORCALL PTSIMDM44FStoreA(PTM44F* pDestination, PTSIMDMatrix M)
{
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[0].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[1].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[2].x) % 16U) == 0U);
	assert((reinterpret_cast<uintptr_t>(&pDestination->r[3].x) % 16U) == 0U);

	::_mm_store_ps(&pDestination->r[0].x, M.r[0]);
	::_mm_store_ps(&pDestination->r[1].x, M.r[1]);
	::_mm_store_ps(&pDestination->r[2].x, M.r[2]);
	::_mm_store_ps(&pDestination->r[3].x, M.r[3]);
}

inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44FTranspose(PTSIMDMatrix M)
{
	__m128 vTemp1 = ::_mm_shuffle_ps(M.r[0], M.r[1], _MM_SHUFFLE(1, 0, 1, 0));//x0 y0 x1 y1
	__m128 vTemp2 = ::_mm_shuffle_ps(M.r[2], M.r[3], _MM_SHUFFLE(1, 0, 1, 0));//x2 y2 x3 y3
	__m128 vTemp3 = ::_mm_shuffle_ps(M.r[0], M.r[1], _MM_SHUFFLE(3, 2, 3, 2));//z0 w0 z1 w1
	__m128 vTemp4 = ::_mm_shuffle_ps(M.r[2], M.r[3], _MM_SHUFFLE(3, 2, 3, 2));//z2 w2 z3 w3

	PTSIMDMatrix mResult;
	mResult.r[0] = ::_mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(2, 0, 2, 0));//x0 x1 x2 x3
	mResult.r[1] = ::_mm_shuffle_ps(vTemp1, vTemp2, _MM_SHUFFLE(3, 1, 3, 1));//y0 y1 y2 y3
	mResult.r[2] = ::_mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(2, 0, 2, 0));//z0 z1 z2 z3
	mResult.r[3] = ::_mm_shuffle_ps(vTemp3, vTemp4, _MM_SHUFFLE(3, 1, 3, 1));//w0 w1 w2 w3
	return mResult;
}

inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44LookToRH(PTSIMDVector EyePosition, PTSIMDVector EyeDirection, PTSIMDVector UpDirection)
{
	//基变换
	//正交阵的逆为正交阵的转置
	PTSIMDVector NegEyeDirection = ::PTSIMDV4FNegate(EyeDirection);
	PTSIMDVector RZ = ::PTSIMDV3FNormalize(NegEyeDirection);
	PTSIMDVector RX = ::PTSIMDV3FNormalize(::PTSIMDV3FCross(UpDirection, RZ));
	PTSIMDVector RY = ::PTSIMDV3FCross(RZ, RX);

	//平移
	PTSIMDVector NegEyePosition = ::PTSIMDV4FNegate(EyePosition);
	PTSIMDVector DX = ::PTSIMDV3FDot(RX, NegEyePosition);
	PTSIMDVector DY = ::PTSIMDV3FDot(RY, NegEyePosition);
	PTSIMDVector DZ = ::PTSIMDV3FDot(RZ, NegEyePosition);

	PTSIMDMatrix mResult;
	alignas(16) uint32_t uSelect1110[4] = { 0XFFFFFFFFU ,0XFFFFFFFFU ,0XFFFFFFFFU ,0X00000000U };
	__m128 vSelect1110 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect1110));
	mResult.r[0] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, RX.v),//RX x y z _
		::_mm_andnot_ps(vSelect1110, DX.v)//DX _ _ _ w
	);
	mResult.r[1] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, RY.v),//RY x y z _
		::_mm_andnot_ps(vSelect1110, DY.v)//DY _ _ _ w
	);
	mResult.r[2] = ::_mm_or_ps(
		::_mm_and_ps(vSelect1110, RZ.v),//RZ x y z _
		::_mm_andnot_ps(vSelect1110, DZ.v)//DZ _ _ _ w
	);
	mResult.r[3] = ::_mm_set_ps(1.0f, 0.0f, 0.0f, 0.0f);
	return mResult;
}


inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44PerspectiveFovRH(float FovAngleY, float AspectHByW, float NearZ, float FarZ)
{
	//投影面为z=1.0f
	float HalfViewHeight = ::tanf(0.5f * FovAngleY);
	float HalfViewWidth = HalfViewHeight * AspectHByW;

	return ::PTSIMDM44PerspectiveRH(HalfViewWidth*2.0f, HalfViewHeight*2.0f, NearZ, FarZ);
}

inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44PerspectiveRH(float ViewWidth, float ViewHeight, float NearZ, float FarZ)
{
	//Nvidia Reversed-Z Trick
	//http://developer.nvidia.com/content/depth-precision-visualized

	// _  0  0  0
	// 0  _  0  0
	// 0  0  b  a
	// 0  0 -1  0
	
	float b = NearZ / (FarZ - NearZ);
	float a = FarZ / (FarZ - NearZ);//确保FarZ趋向于正无穷大时的正确性
	a *= NearZ;
	alignas(16) float fValues[4] = { 2.0f / ViewWidth,2.0f / ViewHeight,b,a };
	__m128 vValues = ::_mm_load_ps(fValues);//2/Width 2/Height b a

	PTSIMDMatrix mResult;
	//r0
	mResult.r[0] = ::_mm_setzero_ps();
	mResult.r[0] = ::_mm_move_ss(mResult.r[0], vValues);//2/Width 0 0 0
	//r1
	alignas(16) uint32_t uSelect0100[4] = { 0x00000000 ,0xFFFFFFFF ,0x00000000 ,0x00000000 };
	__m128 vSelect0100 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect0100));
	mResult.r[1] = ::_mm_and_ps(vSelect0100, vValues);//0 2/Height 0 0
	//r2
	mResult.r[2] = ::_mm_setzero_ps();
	mResult.r[2] = ::_mm_shuffle_ps(mResult.r[2], vValues, _MM_SHUFFLE(3, 2, 0, 0));//0 0 b a
	//r4
	alignas(16) float fSelect0010[4] = { 0.0f, 0.0f, -1.0f , 0.0f };
	mResult.r[3] = ::_mm_load_ps(fSelect0010);//0 0 -1 0

	return mResult;
}

inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44OrthographicRH(float ViewWidth, float ViewHeight, float NearZ, float FarZ)
{
	//Nvidia Reversed-Z Trick
	//http://developer.nvidia.com/content/depth-precision-visualized

	// _ 0 0 0
	// 0 _ 0 0
	// 0 0 b a
	// 0 0 0 1
	float b = 1.0f / (FarZ - NearZ);
	float a = FarZ / (FarZ - NearZ);//确保FarZ趋向于正无穷大时的正确性
	alignas(16) float fValues[4] = { 2.0f / ViewWidth,2.0f / ViewHeight,b,a };
	__m128 vValues = ::_mm_load_ps(fValues);//2/Width 2/Height b a

	PTSIMDMatrix mResult;
	//r0
	mResult.r[0] = ::_mm_setzero_ps();
	mResult.r[0] = ::_mm_move_ss(mResult.r[0], vValues);//2/Width 0 0 0
	//r1
	alignas(16) uint32_t uSelect0100[4] = { 0x00000000 ,0xFFFFFFFF ,0x00000000 ,0x00000000 };
	__m128 vSelect0100 = ::_mm_load_ps(reinterpret_cast<float *>(uSelect0100));
	mResult.r[1] = ::_mm_and_ps(vSelect0100, vValues);//0 2/Height 0 0
	//r2
	mResult.r[2] = ::_mm_setzero_ps();
	mResult.r[2] = ::_mm_shuffle_ps(mResult.r[2], vValues, _MM_SHUFFLE(3, 2, 0, 0));//0 0 b a
	//r3
	alignas(16) float fSelect0010[4] = { 0.0f, 0.0f, 0.0f , 1.0f };
	mResult.r[3] = ::_mm_load_ps(fSelect0010);//0 0 0 1
	return mResult;
}


inline PTSIMDMatrix PT_VECTORCALL PTSIMDM44Multiply(PTSIMDMatrix M1, PTSIMDMatrix M2)
{
	PTSIMDMatrix mResult;
	__m128 vR = M1.r[0];//a00 a01 a02 a03
	__m128 vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));//a00 a00 a00 a00
	__m128 vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));//a01 a01 a01 a01
	__m128 vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));//a02 a02 a02 a02
	__m128 vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));//a03 a03 a03 a03
	
	//计算第一行
	//映射
	vX = ::_mm_mul_ps(vX, M2.r[0]);//a00*b00 a00*b01 a00*b02 a00*b03
	vY = ::_mm_mul_ps(vY, M2.r[1]);//a01*b10 a01*b11 a01*b12 a01*b13
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);//a02*b20 a02*b21 a02*b22 a02*b23
	vW = ::_mm_mul_ps(vW, M2.r[3]);//a03*b30 a03*b31 a03*b32 a03*b33
	//归约
	mResult.r[0] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));//a00*b00+a01*b10+a02*b20+a03*b30 a00*b01+a01*b11+a02*b21+a03*b31 a00*b02+a01*b12+a02*b22+a03*b32 a00*b03+a01*b13+a02*b23+a03*b33
	
	//计算其它三行
	vR = M1.r[1];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[1] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));
	
	vR = M1.r[2];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[2] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));

	vR = M1.r[3];
	vX = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(0, 0, 0, 0));
	vY = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(1, 1, 1, 1));
	vZ = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(2, 2, 2, 2));
	vW = ::_mm_shuffle_ps(vR, vR, _MM_SHUFFLE(3, 3, 3, 3));
	vX = ::_mm_mul_ps(vX, M2.r[0]);
	vY = ::_mm_mul_ps(vY, M2.r[1]);
	vZ = ::_mm_mul_ps(vZ, M2.r[2]);
	vW = ::_mm_mul_ps(vW, M2.r[3]);
	mResult.r[3] = ::_mm_add_ps(::_mm_add_ps(vX, vZ), ::_mm_add_ps(vY, vW));

	return mResult;
}

//向量(a,b,c,d)表示平面ax + by + cz + d = 0
//点P(px,py,pc)到平面上任意一点在平面法向量(a,b,c)上的投影为(a * xp + b * yp + c * pc + d) / 根号(a^2 + b^2 + c^2)，特别地，当平面法向量为单位向量时，根号(a^2 + b^2 + c^2)为1，上式即为(px,py,pc,1)点积(a,b,c,d)
//可根据投影与0的大小关系确定点P在平面的哪一侧

inline __m128 PT_VECTORCALL PTSIMDInternalPlaneNormalize(__m128 P)
{
	//计算(P.x,P.y,P.z)的长度
	PTSIMDVector vLengthSq = ::PTSIMDV3FDot(PTSIMDVector{ P }, PTSIMDVector{ P });
	__m128 vLength = _mm_sqrt_ps(vLengthSq.v);

	//零向量的方向是任意的！！！
	__m128 vResult = _mm_div_ps(P, vLength);

	return vResult;
}

inline PTSIMDFrustum PT_VECTORCALL PTSIMDFrustumLoadRH(PTSIMDMatrix ViewProjection)
{
	//Gil Gribb  Klaus Hartmann,"Fast Extraction of Viewing Frustum Planes from the WorldView-Projection Matrix", 06/15/2001
	//http://gamedevs.org/uploads/fast-extraction-viewing-frustum-planes-from-world-view-projection-matrix.pdf

	PTSIMDFrustum F;

	//Near
	F.Plane[0] = ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[2]);
	F.Plane[0] = ::PTSIMDInternalPlaneNormalize(F.Plane[0]);

	//Far
	F.Plane[1] = ViewProjection.r[2];
	F.Plane[1] = ::PTSIMDInternalPlaneNormalize(F.Plane[1]);

	//Right
	F.Plane[2]= ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[0]);
	F.Plane[2] = ::PTSIMDInternalPlaneNormalize(F.Plane[2]);

	//Left
	F.Plane[3] = ::_mm_add_ps(ViewProjection.r[3], ViewProjection.r[0]);
	F.Plane[3] = ::PTSIMDInternalPlaneNormalize(F.Plane[3]);

	//Top
	F.Plane[4] = ::_mm_sub_ps(ViewProjection.r[3], ViewProjection.r[1]);
	F.Plane[4] = ::PTSIMDInternalPlaneNormalize(F.Plane[4]);

	//Bottom
	F.Plane[5] = ::_mm_add_ps(ViewProjection.r[3], ViewProjection.r[1]);
	F.Plane[5] = ::PTSIMDInternalPlaneNormalize(F.Plane[5]);

	return F;
}

inline PTSIMDSphere PT_VECTORCALL PTSIMDSphereLoad(PTV3F *pCenter, float Radius)
{
	PTSIMDSphere S;
	S.Center = ::_mm_set_ps(1.0f, pCenter->z, pCenter->y, pCenter->x);
	S.Radius = ::_mm_set_ps1(Radius);
	return S;
}

inline PTSIMDAAB PT_VECTORCALL PTSIMDAABBLoad(PTV3F *pMin, PTV3F *pMax)
{
	PTSIMDAAB AAB;
	__m128 vMin= ::_mm_set_ps(1.0f, pMin->z, pMin->y, pMin->x);
	__m128 vMax = ::_mm_set_ps(1.0f, pMax->z, pMax->y, pMax->x);
	__m128 vTwo = ::_mm_set_ps1(2.0f);
	AAB.Center = ::_mm_add_ps(vMin, vMax);
	AAB.Center = ::_mm_div_ps(AAB.Center, vTwo);
	AAB.HalfDiagonal= ::_mm_sub_ps(vMax, vMin);
	AAB.HalfDiagonal = ::_mm_div_ps(AAB.HalfDiagonal, vTwo);
	return AAB;
}

inline void PT_VECTORCALL PTSIMDInternalPlaneSphereIntersect(__m128 P, PTSIMDSphere S, __m128 &Inside, __m128 &OutSide)
{
	//球心到平面上任意一点在平面法向量上的投影
	PTSIMDVector vD = ::PTSIMDV4FDot(PTSIMDVector{ P }, PTSIMDVector{ S.Center });

	//与PTSIMDFrustumLoadRH一致
	Inside = ::_mm_cmpgt_ps(vD.v, S.Radius);
	OutSide = ::_mm_cmplt_ps(vD.v, ::PTSIMDV4FNegate(PTSIMDVector{ S.Radius }).v);
}

inline PTIntersectionType PT_VECTORCALL PTSIMDFrustumSphereIntersect(PTSIMDFrustum F, PTSIMDSphere S)
{
	__m128 AnyOutside;
	__m128 AllInside;

	__m128 OutSide;
	__m128 Inside;

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[0], S, Inside, OutSide);
	AnyOutside = OutSide;
	AllInside = Inside;

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[1], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[2], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[3], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[4], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneSphereIntersect(F.Plane[5], S, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	if (::_mm_movemask_ps(AnyOutside))//==0XF
	{
		return PTDISJOINT;
	}
	else if (::_mm_movemask_ps(AllInside))//==0XF
	{
		return PTCONTAIN;
	}
	else
	{
		return PTINTERSECT;
	}
}

inline void PT_VECTORCALL PTSIMDInternalPlaneAABIntersect(__m128 P, PTSIMDAAB AAB, __m128 &Inside, __m128 &OutSide)
{
	//长方体中心到平面上任意一点在平面法向量上的投影
	PTSIMDVector vD = ::PTSIMDV4FDot(PTSIMDVector{ P }, PTSIMDVector{ AAB.Center });

	//长方体8个半对角线在平面法向量上的投影的最大值
	//映射
	__m128 vTemp1 = _mm_mul_ps(P, AAB.HalfDiagonal);//逐分量相乘
	__m128 vTemp2 = _mm_setzero_ps();
	vTemp2 = _mm_sub_ps(vTemp2, vTemp1);
	vTemp1 = _mm_max_ps(vTemp1, vTemp2);//取绝对值
	//归约
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 2));//z _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z y _ _ 注意是ss而不是ps!!!
	vTemp2 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 1));//y _ _ _
	vTemp1 = ::_mm_add_ss(vTemp1, vTemp2);//x+z+y _ _ _

	vTemp1 = ::_mm_shuffle_ps(vTemp1, vTemp1, _MM_SHUFFLE(0, 0, 0, 0));//x+z+y x+z+y x+z+y x+z+y

	//与PTSIMDFrustumLoadRH一致
	Inside = ::_mm_cmpgt_ps(vD.v, vTemp1);
	OutSide = ::_mm_cmplt_ps(vD.v, ::PTSIMDV4FNegate(PTSIMDVector{ vTemp1 }).v);
}

inline PTIntersectionType PT_VECTORCALL PTSIMDFrustumAABIntersect(PTSIMDFrustum F, PTSIMDAAB AAB)
{
	__m128 AnyOutside;
	__m128 AllInside;

	__m128 OutSide;
	__m128 Inside;

	::PTSIMDInternalPlaneAABIntersect(F.Plane[0], AAB, Inside, OutSide);
	AnyOutside = OutSide;
	AllInside = Inside;

	::PTSIMDInternalPlaneAABIntersect(F.Plane[1], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[2], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[3], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[4], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	::PTSIMDInternalPlaneAABIntersect(F.Plane[5], AAB, Inside, OutSide);
	AnyOutside = ::_mm_or_ps(AnyOutside, OutSide);
	AllInside = ::_mm_and_ps(AllInside, Inside);

	if (::_mm_movemask_ps(AnyOutside))
	{
		return PTDISJOINT;
	}
	else if (::_mm_movemask_ps(AllInside))
	{
		return PTCONTAIN;
	}
	else
	{
		return PTINTERSECT;
	}
}